/**
 * @file BFS.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief BFS类
 */

#include "BFS.h"
#include <cmath>
#include "Map.h"
#include "Params.h"
#include <cstring>

/**
 * @brief 移动一步
 * @param dir 移动方向
*/
void BFSNode::moveStep(int dir) {
    switch(dir) {
        case Direction::RIGHT:
            this->x += 1;
            break;
        case Direction::UP_RIGHT:
            this->y += 1;
            this->x += 1;
            break;
        case Direction::UP:
            this->y += 1;
            break;
        case Direction::UP_LEFT:
            this->y += 1;
            this->x -= 1;
            break;
        case Direction::LEFT:
            this->x -= 1;
            break;
        case Direction::DOWN_LEFT:
            this->y -= 1;
            this->x -= 1;
            break;
        case Direction::DOWN:
            this->y -= 1;
            break;
        case Direction::DOWN_RIGHT:
            this->y -= 1;
            this->x += 1;
            break;
        default:
            break;
    }
}

/**
 * @brief 创建BFS结点
 * @param x 横坐标
 * @param y 纵坐标
 * @param cost 已有代价
 * @param parentDir 父节点移动方向
 * @param dir 移动方向
 */
BFSNode::BFSNode(int x, int y, double cost, int parentDir, int dir) : x(x), y(y), cost(cost), dir(dir) {
    this->moveStep(this->dir);
    this->cost += Direction::GetDirectionDiff(this->dir, parentDir) * PARAMS.ROTATE_COST;
}

/**
 * @brief 获得躲避的指令序列
 * @param x 起点横坐标
 * @param y 起点纵坐标
 * @param isEmpty 是否使用空载地图
 * @param dispatcher 需要躲避的机器人的调度器
 * @param minSteps 最小检查步数
 * @return std::list<Cmd> 指令序列
 */
std::list<Cmd> BFS::getAvoidCmdSeq(int x, int y, bool isEmpty, Dispatcher *dispatcher, int minSteps) {
    bool visited[MAP_X_MAX_EXPAND][MAP_Y_MAX_EXPAND];   // 节点是否已访问
    std::memset(visited, false, sizeof visited);

    visited[x][y] = true;

    std::list<BFSNode *> openSet; // 开集
    std::list<BFSNode *> closeSet; // 闭集

    BFSNode *root = new BFSNode(x, y, 0, Direction::NONE, Direction::NONE); // 路径树
    BFSNode *current = root; // 当前结点
    BFSNode *child = nullptr; // 孩子结点

    std::list<BFSNode *>::iterator minCostNode = openSet.begin(); // 最小代价结点
    bool isComplete = false; // 已完成寻路
    while(true) {
        // 将当前结点周围能走的点加入开集
        for(int dir = 1; dir <= 8; ++dir) { // 0方向为原地不动
            child = new BFSNode(current->x, current->y, current->cost, current->dir, dir);
            if(Map::isInMap(child->x, child->y) && !Map::isObstacle(child->x, child->y, isEmpty) && !visited[child->x][child->y]) {
                int avoidDis = dispatcher->isInPath(child->x, child->y, isEmpty, minSteps);
                if(avoidDis == -1 || Direction::GetDirectionDiff(avoidDis, current->dir) <= 3) { // 该点不在路径上或不与躲避对象对向行驶
                    double inPathCost; // 在路径上行驶的代价系数
                    switch(avoidDis) {
                        default:
                        case -1:
                            inPathCost = 0;
                            break;
                        case 0: // 同向
                            inPathCost = 1;
                            break;
                        case 1: // 同向斜向
                            inPathCost = 0.75;
                            break;
                        case 2: // 侧向
                            inPathCost = 0.5;
                            break;
                        case 3: // 对向斜向
                            inPathCost = 1.75;
                            break;
                        case 4: // 对向
                            inPathCost = 2;
                            break;
                    }
                    child->cost += inPathCost * PARAMS.IN_PATH_COST;
                    // 入树
                    current->childs.push_back(child);
                    child->parent = current;
                    // 加入开集
                    openSet.push_back(child);
                    // 标记走过
                    visited[child->x][child->y] = true;
                }
            } else {
                delete child;
                child = nullptr;
            }
        }

        // 开集为空，说明寻路失败
        if(openSet.empty()) {
            break;
        }

        // 将当前结点加入闭集
        closeSet.push_back(current);

        // 取出开集中最小代价点
        if(!openSet.empty()) {
            minCostNode = openSet.begin();
            for(std::list<BFSNode *>::iterator it = openSet.begin(); it != openSet.end(); ++it){
                if((*it)->cost < (*minCostNode)->cost) {
                    minCostNode = it;
                }
            }
            current = *minCostNode;
            openSet.erase(minCostNode);
        }

        // 判断寻路是否结束
        if(dispatcher->isInPath(current->x, current->y, isEmpty, minSteps) == -1) {
            isComplete = true;
            break;
        }
    }

    std::list<Cmd> cmdSeq; // 指令序列
    // 创建指令并压入指令序列
    if(isComplete) {
        std::list<Cmd> tmpSeq; // 临时指令序列
        current = current->parent; // 第一步为原地，不加入指令序列
        while(current){
            tmpSeq.push_front(Cmd::createRunTo(current->x, current->y, current->dir, isEmpty));
            current = current->parent;
        }
        cmdSeq.splice(cmdSeq.end(), tmpSeq);
    }

    // 清空开集
    while(!openSet.empty()) {
        delete openSet.back();
        openSet.pop_back();
    }
    // 清空闭集
    while(!closeSet.empty()) {
        delete closeSet.back();
        closeSet.pop_back();
    }

    return cmdSeq;
}
